#pragma once

#include <boost/numeric/ublas/vector.hpp>
#include "SimResult.h"
#include "filter.h"

class Environment;
class Track;

class Robomag : public Robot {
	typedef boost::numeric::ublas::vector<double> vec;

public:	
	double MaxSpeed;		// maximum speed in cm/s
	double MaxWheelAngle; 	// maximum wheel angle in radians
	double Width;			// width of wheelbase
	double Length;			// length of wheelbase
	double CenterOfMass;	// distance of center of mass from rear axle
	
	double Kp;				// PID parameters
	double Ki;
	double Kd;

	static const int NUM_GOALS = 4;
	vec goals[NUM_GOALS];

public:

	Robomag();

	~Robomag();

	void Start();

	// Every step, we are allowed to update our speed and direction, that's it.
	void Step(double t, double dt);

	// Save current state in the simulation result
	void Save(SimResult::Data &data);

	// gets the speed of the robot in cm/s.
	// sets the speed by clamping within range
	double GetSpeed() const { return mSpeed * MaxSpeed; }

	// returns the angle of the wheels
	// sets the wheel angle by claming within max range
	double GetWheelAngle() const { return mDirection * MaxWheelAngle; }

	double GetCenterOfMass() const { return CenterOfMass; }

	double GetWidth() const { return Width; }
	
	double GetLength() const { return Length; }

	void SetEnvironment(Environment *env) { mEnv = env; } 

private:
	void SetSpeed(double value);

	void SetWheelAngle(double value);
	
private:
	Environment *mEnv;

	double mSpeed;								// [0, 1.0]. 0 is stopped, 1.0 is full speed
	double mDirection;							// [-1.0, 1.0]. -1.0 is all the way left, 1.0 is all the way right.
	double mPreviousError;
	double mIntegral;
	double mError;
	double mOutput;

	bool mHasReachedGoal;
	int mCurGoal;
};
